#include "tbcore/geo/conv.hpp"

//fix srm's import library bug
#ifndef BUILD_SRM_CPP
#define BUILD_SRM_CPP
#endif

#include <srm/srm_types.h>
#include <srm/Celestiodetic.h>
#include <srm/Celestiocentric.h>
#include <srm/LocalTangentSpaceEuclidean.h>

#include "tbcore/base/tostring.hpp"
#include "tbcore/math/math.hpp"

TB_NAMESPACE_BEGIN

namespace geo {

const SRM_Integer ORM_RT_RELATIONSHIP_TABLE[][2] = {
	{ SRM_ORMCOD_WGS_1984, SRM_RTCOD_WGS_1984_IDENTITY }
};

const uint32 SIZE_OF_ORM_RT_RELATIONSHIP_TABLE = TB_ARRAYSIZE(ORM_RT_RELATIONSHIP_TABLE);

SRM_Integer GetRT(SRM_Integer ormcod) {
	for (uint32 i = 0; i < SIZE_OF_ORM_RT_RELATIONSHIP_TABLE; ++i) {
		if (ORM_RT_RELATIONSHIP_TABLE[i][0] == ormcod) {
			return ORM_RT_RELATIONSHIP_TABLE[i][1];
		}
	}
	return SRM_RTCOD_UNSPECIFIED;
}

Result GCCToLLZ(const GCCCoord& src, LLZCoord& dest) {
  try {
    srm::SRF_Celestiocentric* srcsrf = srm::SRF_Celestiocentric::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CC srcCoord(srcsrf, src.x, src.y, src.z);

		srm::SRF_Celestiodetic* dessrf = srm::SRF_Celestiodetic::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CD desCoord(dessrf);

    srcsrf->changeCoordinate3DSRF(srcCoord, desCoord);
    dest.orm = src.orm;
    dest.lon = desCoord.get_longitude();
    dest.lat = desCoord.get_latitude();
    dest.z = desCoord.get_ellipsoidal_height();

    srcsrf->release();
    dessrf->release();
  } catch (srm::Exception& e) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(e.getWhat())));
  }

	return Result();
}

Result GCCToGCS(const GCCCoord& src, GCSCoord& dest) {
  try {
	  srm::SRF_Celestiocentric* srcsrf = srm::SRF_Celestiocentric::create(src.orm, GetRT(src.orm));
	  srm::Coord3D_CC srcCoord(srcsrf, src.x, src.y, src.z);

	  SRM_SRFS_Info gtrs_gcs_srf_params;
	  gtrs_gcs_srf_params.srfs_code_info.srfs_code = SRM_SRFSCOD_GTRS_GLOBAL_COORDINATE_SYSTEM;
	  gtrs_gcs_srf_params.srfs_code_info.value.srfsm_gtrs_gcs = dest.cell;
	  gtrs_gcs_srf_params.orm_code = src.orm;

	  srm::SRF_LocalTangentSpaceEuclidean* dessrf = (srm::SRF_LocalTangentSpaceEuclidean*)srm::BaseSRF::createSRFSetMember
			(gtrs_gcs_srf_params, GetRT(src.orm));

	  srm::Coord3D_LTSE desCoord(dessrf);

	  srcsrf->changeCoordinate3DSRF(srcCoord, desCoord);
	  dest.orm = src.orm;
	  dest.x = desCoord.get_x();
	  dest.y = desCoord.get_y();
	  dest.z = desCoord.get_height();

	  srcsrf->release();
	  dessrf->release();
  } catch (srm::Exception& ex) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(ex.getWhat())));
  }

	return Result();
}

Result LLZToGCC(const LLZCoord& src, GCCCoord& dest) {
  try {
    srm::SRF_Celestiodetic* srcsrf = srm::SRF_Celestiodetic::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CD srcCoord(srcsrf, src.lon, src.lat, src.z);

		srm::SRF_Celestiocentric* dessrf = srm::SRF_Celestiocentric::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CC desCoord(dessrf);

	  dessrf->changeCoordinate3DSRF(srcCoord, desCoord);

		dest.orm = src.orm;
    dest.x = desCoord.get_u();
    dest.y = desCoord.get_v();
    dest.z = desCoord.get_w();

    srcsrf->release();
    dessrf->release();
  } catch (srm::Exception& e) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(e.getWhat())));
  }

	return Result();
}

Result LLZToGCS(const LLZCoord& src, GCSCoord& dest) {
  try {
	  srm::SRF_Celestiodetic* srcsrf = srm::SRF_Celestiodetic::create(src.orm, GetRT(src.orm));
	  srm::Coord3D_CD srcCoord(srcsrf, src.lon, src.lat, src.z);

	  SRM_SRFS_Info gtrs_gcs_srf_params;
	  gtrs_gcs_srf_params.srfs_code_info.srfs_code = SRM_SRFSCOD_GTRS_GLOBAL_COORDINATE_SYSTEM;
	  gtrs_gcs_srf_params.srfs_code_info.value.srfsm_gtrs_gcs = dest.cell;
		gtrs_gcs_srf_params.orm_code = src.orm;

	  srm::SRF_LocalTangentSpaceEuclidean* dessrf = (srm::SRF_LocalTangentSpaceEuclidean*)srm::BaseSRF::createSRFSetMember
			(gtrs_gcs_srf_params, GetRT(src.orm));

	  srm::Coord3D_LTSE desCoord(dessrf);

	  srcsrf->changeCoordinate3DSRF(srcCoord, desCoord);
	  dest.orm = src.orm;
	  dest.x = desCoord.get_x();
	  dest.y = desCoord.get_y();
	  dest.z = desCoord.get_height();

	  srcsrf->release();
	  dessrf->release();
  } catch (srm::Exception& e) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(e.getWhat())));
  }

	return Result();
}

Result GCSToGCC(const GCSCoord& src, GCCCoord& dest) {
  try {
    SRM_SRFS_Info gtrs_gcs_srf_params;
    gtrs_gcs_srf_params.srfs_code_info.srfs_code = SRM_SRFSCOD_GTRS_GLOBAL_COORDINATE_SYSTEM;
    gtrs_gcs_srf_params.srfs_code_info.value.srfsm_gtrs_gcs = src.cell;
    gtrs_gcs_srf_params.orm_code = src.orm;

    srm::SRF_LocalTangentSpaceEuclidean* srcsrf = (srm::SRF_LocalTangentSpaceEuclidean*)srm::BaseSRF::createSRFSetMember
      (gtrs_gcs_srf_params, GetRT(src.orm));

    srm::Coord3D_LTSE srcCoord(srcsrf, src.x, src.y, src.z);

    srm::SRF_Celestiocentric* dessrf = srm::SRF_Celestiocentric::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CC desCoord(dessrf);

    srcsrf->changeCoordinate3DSRF(srcCoord, desCoord);
    dest.orm = src.orm;
    dest.x = desCoord.get_u();
    dest.y = desCoord.get_v();
    dest.z = desCoord.get_w();

    srcsrf->release();
    dessrf->release();
  } catch (srm::Exception& e) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(e.getWhat())));
  }

	return Result();
}

Result GCSToLLZ(const GCSCoord& src, LLZCoord& dest) {
  try {
    SRM_SRFS_Info gtrs_gcs_srf_params;
    gtrs_gcs_srf_params.srfs_code_info.srfs_code = SRM_SRFSCOD_GTRS_GLOBAL_COORDINATE_SYSTEM;
    gtrs_gcs_srf_params.srfs_code_info.value.srfsm_gtrs_gcs = src.cell;
    gtrs_gcs_srf_params.orm_code = src.orm;

    srm::SRF_LocalTangentSpaceEuclidean* srcsrf = (srm::SRF_LocalTangentSpaceEuclidean*)srm::BaseSRF::createSRFSetMember
      (gtrs_gcs_srf_params, GetRT(src.orm));

    srm::Coord3D_LTSE srcCoord(srcsrf, src.x, src.y, src.z);

    srm::SRF_Celestiodetic* dessrf = srm::SRF_Celestiodetic::create(src.orm, GetRT(src.orm));
    srm::Coord3D_CD desCoord(dessrf);

    srcsrf->changeCoordinate3DSRF(srcCoord, desCoord);
    dest.orm = src.orm;
    dest.lon = desCoord.get_longitude();
    dest.lat = desCoord.get_latitude();
    dest.z = desCoord.get_ellipsoidal_height();

    srcsrf->release();
    dessrf->release();
  } catch (srm::Exception& e) {
    return Result(kGeoConvertFailed, TB_STRING(std::string(e.getWhat())));
  }

	return Result();
}

// refer: https://en.wikipedia.org/wiki/Geodetic_datum

void ECEF2ENUMatrix(Matrix3d& m, double lon, double lat) {
	double sin_lat = math::Sin(lat);
	double cos_lat = math::Cos(lat);
	double sin_lon = math::Sin(lon);
	double cos_lon = math::Cos(lon);
	m <<	-sin_lon,						cos_lon,						0,
				-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat,
				cos_lat * cos_lon,	cos_lat * sin_lon,	sin_lat;
}

void ECEF2ENUMatrix(Matrix3d& m, const LLZCoord& coord) {
	ECEF2ENUMatrix(m, coord.lon, coord.lat);
}

void ECEF2ENUMatrix(Matrix3d& m, const GCCCoord& src) {
	LLZCoord coord;
	GCCToLLZ(src, coord);
	ECEF2ENUMatrix(m, coord);
}

void ECEF2ENUMatrix(Matrix3d& m, const GCSCoord& src) {
	LLZCoord coord;
	GCSToLLZ(src, coord);
	ECEF2ENUMatrix(m, coord);
}

void ENU2ECEFMatrix(Matrix3d& m, double lon, double lat) {
	double sin_lat = math::Sin(lat);
	double cos_lat = math::Cos(lat);
	double sin_lon = math::Sin(lon);
	double cos_lon = math::Cos(lon);
	m << -sin_lon, -sin_lat * cos_lon, cos_lat * cos_lon,
				cos_lon, -sin_lat * sin_lon, cos_lat * sin_lon,
				0, cos_lat, sin_lat;
}

void ENU2ECEFMatrix(Matrix3d& m, const LLZCoord& coord) {
	ENU2ECEFMatrix(m, coord.lon, coord.lat);
}

void ENU2ECEFMatrix(Matrix3d& m, const GCCCoord& src) {
	LLZCoord coord;
	GCCToLLZ(src, coord);
	ENU2ECEFMatrix(m, coord);
}

void ENU2ECEFMatrix(Matrix3d& m, const GCSCoord& src) {
	LLZCoord coord;
	GCSToLLZ(src, coord);
	ENU2ECEFMatrix(m, coord);
}

} //namespace geo

TB_NAMESPACE_END
